{

Position Controller Test Program

Written by Daniel Harris, David Carrier, Kevin McCulla, Kevin Cook

}
CON

  _clkmode = xtal1 + pll16x
  _clkfreq = 80_000_000


  PIN   =       0               'set this to which pin your position controller is on

  QPOS          = %00001__000
  QSPD          = %00010__000
  CHFA          = %00011__000
  TRVL          = %00100__000
  CLRP          = %00101__000
  SREV          = %00110__000
  STXD          = %00111__000  
  SMAX          = %01000__000
  SSRR          = %01001__000

obj

  mc:   "FullDuplexSerial"

pub go

  dira[1]~~
  mc.start(PIN, PIN, %1100, 19_200)

  
  move_position(-100, 1)       'you can use a negative or positive number here to travel clockwise or counter clockwise
  wait_till_arrival(0, 1)
  move_position(100, 1)
  wait_till_arrival(0, 1)
  

pub move_position(num_pos, position_controller)

  mc.tx(CLRP | position_controller)
  mc.tx(TRVL | position_controller)
  mc.tx(num_pos >> 8)
  mc.tx(num_pos >> 0)

pub wait_till_arrival(tollerance, position_controller)

  repeat
    waitcnt(constant(_clkfreq / 10) + cnt)
    mc.tx(CHFA | position_controller)
    mc.tx(tollerance)
  until mc.rx  
  